#include"ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32MultiArray.h"
#include "geometry_msgs/Pose.h"
#include <stdlib.h>
#define step_length  0.05//步长，越小分点越细

float cur_pose_x = 0.0;
float cur_pose_y = 0.0;
int step = 1;
int cnt = 0;
//回调函数
void locationCallback(const geometry_msgs::Pose::ConstPtr &msg1)
{
   cur_pose_x = msg1->position.x;
   cur_pose_y = msg1->position.y;
   ROS_INFO("get location");
   std::cout<<"pose:x y: "<<msg1->position.x<<" "<<msg1->position.y<<std::endl;
}
int main(int argc, char *argv[])
{
   setlocale(LC_ALL,"");
   ros::init(argc,argv,"path_planning");
   ros::NodeHandle nh;
   ros::Subscriber sub = nh.subscribe("/location",10,locationCallback);//订阅当前位置
   ros::Publisher pub1= nh.advertise<std_msgs::Float32MultiArray>("/mpc_target_point",10);//发布终点坐标
   ros::Publisher pub2= nh.advertise<std_msgs::Float32MultiArray>("/mpc_route",10);//发布路径点

   std_msgs::Float32MultiArray mpc_target_point;
   std_msgs::Float32MultiArray route;
   
   while(ros::ok){
      ros::spinOnce();
      if (cnt == 0)
      {
      	  mpc_target_point.data.push_back(10);
  	  mpc_target_point.data.push_back(10);
  	  cnt++;
  	  cnt = cnt % 4;
      }
      else if (cnt == 1)
      {
      	  mpc_target_point.data.push_back(30);
  	  mpc_target_point.data.push_back(10);
  	  cnt++;
  	  cnt = cnt % 4;
      }
      else if (cnt == 2)
      {
      	  mpc_target_point.data.push_back(30);
  	  mpc_target_point.data.push_back(30);
  	  cnt++;
  	  cnt = cnt % 4;
      }
      else if (cnt == 3)
      {
      	  mpc_target_point.data.push_back(10);
  	  mpc_target_point.data.push_back(30);
  	   cnt++;
  	  cnt = cnt % 4;
      }
      
      route.data.clear();
      try{
      float dX = mpc_target_point.data[0] - cur_pose_x;
      float dY = mpc_target_point.data[1] - cur_pose_y;
      step = static_cast<int>(std::ceil(std::max(std::abs(dX),std::abs(dY)) / step_length));
      if(!step)
      {
         throw 0 ;
      }
      float dx = dX/step;
      float dy = dY/step;
      for(int i=0;i<=step;i++)
      {
          route.data.push_back(cur_pose_x + i*dx);
          route.data.push_back(cur_pose_y + i*dy);
      }
      pub1.publish(mpc_target_point);//发布终点
      pub2.publish(route);           //发布路径点
      }
      catch(...){
         ROS_INFO("Wrong divisor");
         step = 1;
         return 0;
      	}
      mpc_target_point.data.clear();
      ros::Duration sleep_time(10);
      sleep_time.sleep();
   }
   return 0;
}
